/*
 * Sensor.cc
 *
 *  Created on: Sep 22, 2012
 *      Author: KHANH HUNG
 */

#include "Sensor.h"
#include "Target.h"
#include <time.h>
#include <stdlib.h>

Define_Module(Sensor);

void Sensor::initialize()
{
    this->myID                  = this->getId();
    this->active                = false;
    this->isClusterHead         = false;
    this->trRange               = par("trRange");
    this->ssRange               = par("ssRange");
    this->infoTarget            = 0;
    this->LastModuleId          = simulation.getLastModuleId();
    this->isDead                = false;
    this->isRelayNode           = false;
    this->relayNode             = NULL;
    this->totalEventModule      = 0;
    this->totalStatusInfoMsg    = 0;
    this->numDataToCH           = 0;
    this->hasLinks              = false;
    this->hasPath               = false;
    this->frameNumber           = par("frameNumber");
    this->countPackageTransmit  = 0;
    this->bs                    = (Bastation *)simulation.getModule(LastModuleId);
    this->tar1                  = simulation.getModule(2);
    sigmaMeasurementNoise       = 0.1 * ssRange / 2.5;
    this->noise                 = GaussianNoise(0, sigmaMeasurementNoise);

    //KF
    this->hasOldInfoTarget      = false;
    this->timeStep              = getParentModule()->par("TimeStep");
    this->processNoiseIntensity = getParentModule()->par("ProcessNoiseIntensity");
    KF                          = Kalman(timeStep, processNoiseIntensity);
    this->X                     = Matrix(4, 1, 0);
    this->P                     = Matrix(4, 4, 1);

    //EMRP
    this->switchLevel           = getParentModule()->par("switchLevel");
    this->backupNode            = NULL;
    this->relayEnergy           = 0;
    this->backupEnergy          = 0;
    this->relayEnergyEstimate   = 0;

    //CSMA paramater
    this->macMinBE              = 3;
    this->macMaxCSMABackoffs    = 5;
    this->aMaxBE                = 5;
    this->BE                    = macMinBE;
    this->NB                    = 0;
    this->symbol                = 0.000016; //s
    this->bitRate               = 250000; //bps
    this->CCAduration           = 8 * symbol;
    this->aUnitBackoffPeriod    = 20 * symbol;  //0.00032s
    this->aMinLIFSPeriod        = 40 * symbol;   //0.00064s
    this->aMinSIFSPeriod        = 12 * symbol; //0.000192
    this->aTurnAroundTime       = 12 * symbol;
    this->countIdError          = 0;
    this->flag                  = 0;

    this->timeOutReceiveAck     = pow(2, aMaxBE) * aUnitBackoffPeriod + CCAduration + ControlMsgLength / bitRate + symbol;
    timeoutREQ_CH               = new TimeOut();
    timeoutREQ_CH->setKind(TimeOutReqCHMsg);

    this->STP                   = timeStep;
    this->SDC                   = 0.4;
    this->threshold_detection   = 0.05; //s
    this->speed = getParentModule()->par("targetSpeed");
    this->hasUseFile = ((int)getParentModule()->par("useFile") == 1) ? true : false;
    if(!hasUseFile) this->Tstart = STP * dblrand();

    if(this->getId() == NUMTARGET + 2)
    {
        arrangeNodes();
    }
    findLinks();
    this->typeProtocol = getParentModule()->par("typeProtocol");
    this->typeEstimate = getParentModule()->par("typeEstimate");
    if(typeProtocol == 1) broadcast();

    WATCH(active);
    WATCH(energy);
    WATCH(hasOldInfoTarget);
    WATCH(flag);
    WATCH(totalEventModule);
    WATCH(totalStatusInfoMsg);
    WATCH(hasPath);
    WATCH(isClusterHead);
    WATCH(Tstart);
}

void Sensor::handleMessage(cMessage *msg)
{
    if(msg->isSelfMessage())
    {
        if(msg->getKind() == SMSG_INIT)
        {
            findRelayBackupNode();
            delete msg;
        }
        else if(msg->getKind() == EventNotifyMsg)
        {
            //Nut tham gia su kien thi huy reset
            if(typeEstimate == 1 && scheduleResetMsg != NULL)
            {
                if(scheduleResetMsg->getArrivalTime() >= simTime()) cancelEvent(scheduleResetMsg);
            }
            //store ID of sensor actived, this is for simulation
            ((Target *)tar1)->storeID(this->myID);
            this->active = true;
            this->setDisplay("green");
            this->totalEventModule = ((EventNotify *)msg)->getTotalSensor();

            double R_true = this->distanceCal(((Target *)tar1)->xpos, ((Target *)tar1)->ypos);

            //information of Target, this is only for simulation
            this->infoTarget = 1.0 / R_true;

            double error;
            if(!hasUseFile)
            {
                error = noise.NextGaussian();
                bs->storeValue(error, "data/Noise.txt");
            }
            else error = bs->createNoise();
            this->t_start_sampling = simTime();
            this->R_measurement     = R_true + error;
            if(R_measurement > ssRange) R_measurement = ssRange;
            if(R_measurement < 0) R_measurement = 0;

            if(tDetection <= threshold_detection)  //dat 1 nguong
            {
                StatusInfo *StatusMsg = new StatusInfo();
                StatusMsg->setKind(StatusInfoMsg);
                StatusMsg->setSrcAddress(this->getId());
                StatusMsg->setEnergy(this->energy);
                StatusMsg->setInfo(this->infoTarget);
                StatusMsg->setLength(ControlMsgLength);
                if(typeEstimate == 1) StatusMsg->setHasOldInfoTarget(this->hasOldInfoTarget);
                initCSMA(new Package(StatusMsg, BROADCAST, ssRange * 2));
                backOff();
            }

            Active *inActiveMsg = new Active();
            inActiveMsg->setKind(InActiveSelfMsg);
            scheduleAt(simTime() + timeStep, inActiveMsg);

            delete msg;

        }

        else if(msg->getKind() == BackOffSeflMsg)
        {
            if(this->energy >= 10.0)
            {
                CCA *ccaMsg = new CCA();
                ccaMsg->setKind(CCASeflMsg);
                scheduleAt(simTime() + CCAduration, ccaMsg);
                ev <<"\n Sensor " << myID - (NUMTARGET + 2) <<" CCA";
            }
            else
            {
                this->active = false;
                this->isDead = true;
                this->setDisplay("black");
            }
            delete msg;

        }
        else if(msg->getKind() == CCASeflMsg)
        {
            if(this->energy >= 10.0) clearChannelAssessment();
            else
            {
                this->active = false;
                this->isDead = true;
                this->setDisplay("black");
            }
            delete msg;
        }
        else if(msg->getKind() == CleanChanelSeflMsg)
        {
            CleanChanel *ccMsg = (CleanChanel *)msg;
            this->giaiPhongKenh(ccMsg->getDistance());
            delete msg;
        }
        else if(msg->getKind() == SelfTDMAMsg)
        {
            DataToCH *dataCH = new DataToCH();
            dataCH->setKind(DataToCHMsg);
            dataCH->setLength(DataMsgLength);
            dataCH->setSrcAddress(this->getId());
            dataCH->setDestAddress(this->clusterHead->getId());
            dataCH->setR_measurement(this->R_measurement);

            //if sensor has past Information of target, it will send to CH -> for EKF
            if(hasOldInfoTarget && !clusterHead->hasOldInfoTarget)
            {
                if(this->getId() == ((SelfTDMA *)msg)->getIdSendInfoTarget())
                {
                    ev <<"\n Sensor " << myID - (NUMTARGET + 2) <<" send Target Info to CH";
                    dataCH->setHasOldInfoTarget(this->hasOldInfoTarget);
                    for(int i = 0; i < 4; i++)
                    {
                        dataCH->setX_Old(i, X.getData(i, 0));
                    }

                    int count = 0;
                    for(int i = 0; i < 4; i++)
                        for(int j = 0; j < 4; j++)
                        {
                            if(j >= i)
                            {
                                dataCH->setP_Old(count, P.getData(i, j));
                                count++;
                            }
                        };
                }
                else dataCH->setHasOldInfoTarget(false);
            }
            else dataCH->setHasOldInfoTarget(false);
            char gateName[15];
            sprintf(gateName, "O_%d", this->clusterHead->getId());
            if(this->hasGate(gateName))
            {
                cGate *g = gate(gateName);
                g->getDisplayString().parse("ls=blue,1");
                sendDelayed(dataCH, DataMsgLength / this->bitRate, gateName);
            }
            double d = this->distanceCal(this->clusterHead->xpos, this->clusterHead->ypos);
            this->energySend(DataMsgLength, d);
            delete msg;
        }
        else if(msg->getKind() == CHSelfMsg)
        {
            if(!this->isDead)
            {
                DataToBS *dataBS = new DataToBS();
                dataBS->setKind(DataToBSMsg);
                dataBS->setX(this->X.getData(0, 0));
                dataBS->setY(this->X.getData(2, 0));
                if(typeEstimate == 1)
                {
                    dataBS->setVelocity(sqrt(pow(X.getData(1, 0), 2) + pow(X.getData(3, 0), 2)));
                }
                dataBS->setSrcAddress(myID);
                dataBS->setLength(DataMsgLength);
                QueueMsg.clear();
                QueueMsg.insert(dataBS);

                this->transmitDataToBS();

                if(typeProtocol == 0)
                {
                    if(countPackageTransmit == 1 && this->distanceCal(bs->xpos, bs->ypos) > trRange)
                    {
                        hasPath = false;
                        this->deleteLink(this->relayNode);
                        countPackageTransmit = 0;
                    }
                    countPackageTransmit++;
                }
            }
            else
            {
                bs->frameNumber = par("frameNumber");
            }

            delete msg;
        }
        else if(msg->getKind() == NotifySeflMsg)
        {
            double dBroadcast = ssRange + this->distanceCal(KF.getX0().getData(0, 0), KF.getX0().getData(2, 0));
            if(dBroadcast > trRange) dBroadcast = trRange;
            this->broadcastTargetInfo(dBroadcast);
            delete msg;
        }
        else if(msg->getKind() == CHNotifyActiveMsg)
        {
            double dBroadcast = ssRange + speed * timeStep + this->R_measurement;
            if(dBroadcast > trRange) dBroadcast = trRange;

            Active *activeMsg = (Active *)msg;
            activeMsg->setSrcAddress(this->getId());
            activeMsg->setLength(ControlMsgLength);
            activeMsg->setValue(t_start_sampling);
            initCSMA(new Package(activeMsg, BROADCAST, dBroadcast));
            backOff();
        }
        else if(msg->getKind() == ScheduleResetSeflMsg)
        {
            ev <<"\n Sensor " << myID - 3 <<" xoa thong tin theo lich";
            this->hasOldInfoTarget = false;

            if(this->energy < 10.0)
            {
                this->setDisplay("black");
                this->isDead = true;
                this->active = false;
            }

        }
        else if(msg->getKind() == TimeOutReqRelayMsg)
        {
            if(typeProtocol == 0) this->findRelayNode();
            else if(typeProtocol == 1) this->findRelayBackupNode();

            if(this->hasPath)
            {
                this->relayNode->setDisplay("red");
                this->relayNode->bubble("I'm Relay Node !");
                if(QueueMsg.isEmpty()) return;
                DataToBS *dataBS = (DataToBS *)QueueMsg.pop();
                dataBS->setDestAddress(this->relayNode->getId());

                double d = this->distanceCal(this->relayNode->xpos, this->relayNode->ypos);
                initCSMA(new Package(dataBS->dup(), TRANSMIT, d));
                backOff();
            }

        }
        else if(msg->getKind() == TimeOutReqCHMsg)
        {
            this->totalEventModule = this->totalStatusInfoMsg + 1;
            this->totalStatusInfoMsg = 0;
            ev <<"\n Sensor " << this->getId() - 3 <<" tinh CH";
            if(totalEventModule >= 3) this->electCH();

        }
        else if(msg->getKind() == ActiveSeflMsg)
        {
            if(!this->active)
            {
                this->active = true;
                this->setDisplay("green");
                Active *inActiveMsg = new Active();
                inActiveMsg->setKind(InActiveSelfMsg);
                scheduleAt(((Active *)msg)->getValue() + timeStep, inActiveMsg);
                ev <<"\n Active boi CH";
            }
            delete msg;
        }
        else if(msg->getKind() == InActiveSelfMsg)
        {
            double d = this->distanceCal(((Target *)tar1)->xpos, ((Target *)tar1)->ypos);
            if(d > ssRange)
            {
                this->active = false;
                this->setDisplay("");
                ev <<"\n Inactive";
                if(this->energy < 10)
                {
                    this->isDead = true;
                    this->setDisplay("black");
                }

            }
            else ev <<"\n Actived";

            delete msg;
        }
        else if(msg->getKind() == TimeOutEnergyRelay)
        {
            this->relayEnergy = 0;
            this->relayEnergyEstimate = 0;
            ev <<"\n Relay "<<this->relayNode->getId() <<"\n chet";
        }
    }
    else
    {
        if(msg->getKind() == StatusInfoMsg)
        {
            this->energyReceive(ControlMsgLength);
            if(timeoutREQ_CH->getArrivalTime() >= simTime()) cancelEvent(timeoutREQ_CH);
            double timeOut = (pow(2, 5) - 1) * aUnitBackoffPeriod + CCAduration + ControlMsgLength / bitRate + threshold_detection;
            scheduleAt(simTime() + timeOut, timeoutREQ_CH);
            double d = this->distanceCal(((Target *)tar1)->xpos, ((Target *)tar1)->ypos);
            if(d > ssRange)
            {
                this->active = false;
                this->setDisplay("");
                if(this->energy < 10)
                {
                    this->isDead = true;
                    this->setDisplay("black");
                }

            }

            if(!this->isDead && this->active && this->totalEventModule >= 3)
            {
                int id = ((StatusInfo *)msg)->getSrcAddress();
                this->memberCluster[this->totalStatusInfoMsg] = getSensor(id);
                this->totalStatusInfoMsg++;
            }
            else
            {
                if(this->isDead) this->setDisplay("black");
                this->hasOldInfoTarget = false;
                ev <<"\n Required least 3 Sensor !";

            }
            delete msg;
        }
        else if(msg->getKind() == TDMAMsg)
        {
            this->energyReceive(ControlMsgLength);
            if(this->active)
            {
                TDMA *tdma = (TDMA *)msg;
                int idCH = tdma->getSrcAddress();
                int idSendInfoTarget = tdma->getIdSendInfoTarget();

                //-> Khi CH đã có thông tin về mục tiêu tại thời điểm trước thì các nút trong cụm có sẽ xóa đi để reset
                if(idSendInfoTarget == -1)
                {
                    ev <<"\n Sensor " << myID - 3 <<" xoa thong tin vi CH da co";
                    this->hasOldInfoTarget = false;
                }
                int timeSlot = -1;
                for(int i = 0; i < (int)tdma->getIdArraySize(); i++)
                {
                    if(tdma->getId(i) == myID)
                    {
                        timeSlot = tdma->getTimeSlot(i);
                        ev <<"\n Sensor " << myID - (NUMTARGET + 2) <<" timeSlot = " << timeSlot;
                        break;
                    }
                }

                if(timeSlot != -1)
                {
                    this->clusterHead = getSensor(idCH);
                    SelfTDMA *selfTDMA = new SelfTDMA();
                    selfTDMA->setKind(SelfTDMAMsg);
                    if(typeEstimate == 1) selfTDMA->setIdSendInfoTarget(idSendInfoTarget);
                    scheduleAt(simTime() + timeSlot * DataMsgLength / this->bitRate, selfTDMA);
                }
            }
            else
            {
                //When node is dead, total Event Module decrease 1
                this->clusterHead->totalEventModule--;
            }
            delete msg;

        }
        else if(msg->getKind() == DataToCHMsg)
        {
            this->energyReceive(DataMsgLength);
            this->numDataToCH++;
            if(this->isDead)
            {
                bs->frameNumber = par("frameNumber");
            }
            else if(this->active && this->numDataToCH == this->totalEventModule - 1)
            {
                this->numDataToCH = 0;
                if(typeEstimate == 0) this->locateTarget();  //--> Lateration
                else  //--> EKF
                {
                    if(hasOldInfoTarget)
                    {
                        ev <<"\n CH co thong tin " << this->myID - 3;
                        KF.setX(X);
                        KF.setP(P);
                    }
                    else //if else, CH will get Info from neighbor receive
                    {
                        ev <<"\n CH khong co thong tin " << this->myID - 3;
                        DataToCH *dataCH = (DataToCH *)msg;
                        if(dataCH->getHasOldInfoTarget())
                        {
                            ev <<"\n Nut " << getSensor(dataCH->getSrcAddress())->myID - 3 <<" co thong tin";

                            for(int i = 0; i < 4; i++)
                            {
                                X.setData(i, 0, dataCH->getX_Old(i));
                            }

                            double cM[dataCH->getP_OldArraySize()];
                            for(int i = 0; i < (int)dataCH->getP_OldArraySize(); i++)
                            {
                                cM[i] = dataCH->getP_Old(i);
                            }

                            P = getCovarianceMatrix(cM);
                            KF.setX(X);
                            KF.setP(P);
                            this->hasOldInfoTarget = true;
                        }
                    }

                    //estimate position of target
                    estimatesTarget();

                    X = KF.getX();
                    P = KF.getP();
                    this->hasOldInfoTarget = true;
                    ev <<"\n X_Estimates: "; X.Output();
                    ev <<"\n P_Estimates: "; P.Output();

                }
                this->sendSeflMsgToCH();
            }

            delete msg;
        }
        else if(msg->getKind() == RelayRequestMsg)
        {
            this->energyReceive(ControlMsgLength);
            int IdNodeSend = ((RelayRequest *)msg)->getSrcAddress();
            Sensor *s = getSensor(IdNodeSend);

            double djCH = this->distanceCal(s->xpos, s->ypos);
            double djBS = this->distanceCal(bs->xpos, bs->ypos);
            double dCHBS = s->distanceCal(bs->xpos, bs->ypos);
            double cosa = (djCH * djCH + dCHBS * dCHBS - djBS * djBS)/(2 * djCH * dCHBS);

            if(!s->isDead && cosa >= 0)
            {
                AckRelay *ack = new AckRelay();
                ack->setKind(AckRelayMsg);
                ack->setSrcAddress(this->getId());
                ack->setX(xpos);
                ack->setY(ypos);
                ack->setEnergy(this->energy);
                ack->setLength(ControlMsgLength);
                ack->setDestAddress(IdNodeSend);

                initCSMA(new Package(ack, TRANSMIT, trRange)); //d = trRange: tốn năng lượng nhưng hạn chế lỗi
                backOff();
                ev <<"\n Sensor " << myID - 3 <<" send ";

            }
            delete msg;
        }
        else if(msg->getKind() == AckRelayMsg)
        {
            energyReceive(ControlMsgLength);
            if(timeoutREQ_Relay != NULL && timeoutREQ_Relay->getArrivalTime() >= simTime())
            {
                cancelEvent(timeoutREQ_Relay);
                ev <<"\n cancelEvent(timeOutMsg)";
            }
            timeoutREQ_Relay = new TimeOut();
            timeoutREQ_Relay->setKind(TimeOutReqRelayMsg);
            scheduleAt(simTime() + timeOutReceiveAck, timeoutREQ_Relay);
            ev <<"\n Create time out";

            //bs->storeValue(msg->getSendingTime().dbl(), "data/time.txt");
            if(msgStore != NULL)
            {
                if((msg->getSendingTime() - msgStore->getSendingTime()).dbl() < ControlMsgLength / bitRate + aMinLIFSPeriod)
                {
                    this->storeIdError(msgStore->getSrcAddress(), ((SensorMessage *)msg)->getSrcAddress());
                }
            }
            this->msgStore = (SensorMessage *)msg->dup();
            //bs->storeValue(((SensorMessage *)msg)->getSrcAddress(), "data/id2.txt");


            delete msg;
        }
        else if(msg->getKind() == DataToBSMsg)
        {
            this->energyReceive(DataMsgLength);
            DataToBS *dataBS = (DataToBS *)msg;

            //Xoa màu nút
            Sensor *s = getSensor(dataBS->getSrcAddress());
            if(!s->isClusterHead) s->setDisplay("");

            dataBS->setParentAdress(dataBS->getSrcAddress());
            dataBS->setSrcAddress(this->getId());
            QueueMsg.insert(dataBS);


            ((Target *)tar1)->storeID(this->getId());
            if(typeProtocol == 0) //ARPEES
            {
                countPackageTransmit++;
                if(countPackageTransmit == 1)
                {
                    countPackageTransmit = 0;
                    if(this->relayNode != NULL)
                    {
                        hasPath = false;
                        this->deleteLink(this->relayNode);
                    }
                }
            }
            else if(typeProtocol == 1)
            {
                countPackageTransmit++;
            }

            if(!bs->testAlive())
            {
                endSimulation();
            }
            else
            {
                if(!this->isDead)
                {
                    this->isRelayNode = true;
                    this->transmitDataToBS();
                }
                else
                {
                    bs->frameNumber = par("frameNumber");
                }
            }
        }
        else if(msg->getKind() == EnergyRelayInfoMsg)
        {
            this->energyReceive(ControlMsgLength);
            if(timeoutEnergy_Relay != NULL && timeoutEnergy_Relay->getArrivalTime() >= simTime())
            {
                cancelEvent(timeoutEnergy_Relay);
                ev <<"\n cancelEvent(timeoutEnergy_Relay)";
            }
            this->relayEnergy = ((EnergyRelayInfo *)msg)->getEnergy();
            if(this->relayEnergy < 10.5)  //tìm lại đường
            {
                this->relayEnergy = 0;
            }
            this->relayEnergyEstimate = this->relayEnergy;
            delete msg;
        }
        else if(msg->getKind() == TargetInfoMsg)
        {
            energyReceive(DataMsgLength);
            TargetInfo *tarInfoMsg = (TargetInfo *)msg;
            for(int i = 0; i < 4; i++)
            {
                X.setData(i, 0, tarInfoMsg->getX(i));
            }

            double cM[tarInfoMsg->getPArraySize()];
            for(int i = 0; i < (int)tarInfoMsg->getPArraySize(); i++)
            {
                cM[i] = tarInfoMsg->getP(i);
            }

            P = getCovarianceMatrix(cM);

            KF.setX(X);
            KF.setP(P);
            KF.Predict();
            double d = this->distanceCal(KF.getX0().getData(0, 0), KF.getX0().getData(2, 0));
            if(d <= ssRange)
            {
                this->hasOldInfoTarget = true;
                if(!this->active)
                {
                    Active *activeMsg = new Active();
                    activeMsg->setKind(ActiveSeflMsg);
                    activeMsg->setValue(tarInfoMsg->getValue());
                    scheduleAt(simTime() + Tactive, activeMsg);
                }
                scheduleResetMsg = new ScheduleReset();
                scheduleResetMsg->setKind(ScheduleResetSeflMsg);
                scheduleAt(simTime() + timeStep * 1.5, scheduleResetMsg);
            }

            delete msg;
        }
        else if(msg->getKind() == CHNotifyActiveMsg)
        {
            this->energyReceive(ControlMsgLength);
            if(!this->active)
            {
                Active *activeMsg = new Active();
                activeMsg->setKind(ActiveSeflMsg);
                activeMsg->setValue(((Active *)msg)->getValue());
                scheduleAt(simTime() + Tactive, activeMsg);
            }

            delete msg;
        }
    }

}

/**
 * Arange Nodes
 */
void Sensor::arrangeNodes()
{
    if(!hasUseFile)
    {
        remove("data/Coordinates.txt");
    }
    FILE *f;
    Sensor *s;
    int x, y, id;
    f = fopen("data/Coordinates.txt", "r");
    if(f != NULL)
    {
        for(int i = NUMTARGET + 2; i < LastModuleId; i++)
        {
            s = (Sensor *)simulation.getModule(i);
            fscanf(f, "%d %d %d %lf", &id, &s->xpos, &s->ypos, &s->Tstart);
            s->setDisplay();
        }
    }
    else
    {
        int nSensorPerCol = simulation.getModule(1)->par("nSensorPerCol");
        int rangeMax = simulation.getModule(1)->par("rangeMax");
        f = fopen("data/Coordinates.txt", "w");
        for(int i = NUMTARGET + 2; i < LastModuleId; i++)
        {
            s = (Sensor *)simulation.getModule(i);
            x = (i - NUMTARGET - 2) / nSensorPerCol;
            y = (i - NUMTARGET - 2) % nSensorPerCol;
            s->xpos = x * rangeMax + rand() % ((rangeMax + 1) / 2);
            s->ypos = y * rangeMax + rand() % ((rangeMax + 1) / 2);
            s->Tstart = STP * dblrand();
            fprintf(f, "%3d %3d %3d %6lf \n", i, s->xpos, s->ypos, s->Tstart);
            s->setDisplay();
        }
    }
    fclose(f);
}

/**
 * Khoi tao: Tat ca cac sensor lan luot quang ba thong tin: vi tri, nang luong
 * Khong mo phong giai doan nay, chi tru nang luong cua nut quang ba va nut nhan
 */
void Sensor::broadcast()
{
    //Trừ năng lượng của nút quảng bá
    this->energySend(ControlMsgLength, trRange);

    for(int i = 0; i < numNeighbor; i++)
    {
        //Trừ năng lượng của nút nhận
        trNeighbor[i]->energyReceive(ControlMsgLength);
    }

    //Sau khi tất cả các nút brocast xong, gửi một mesage để các nút tìm Relay và BackUp
    if(this->getId() == simulation.getLastModuleId() - 1)
    {
        for(int i = NUMTARGET + 2; i < bs->getId(); i++)
        {
            getSensor(i)->scheduleAtInit();
        }
    }
}

/**
 * Display position of sensor
 */
void Sensor::setDisplay()
{
    this->getDisplayString().setTagArg("p", 0, this->xpos);
    this->getDisplayString().setTagArg("p", 1, this->ypos);
}

/**
 * Set Display Color of sensor
 */
void Sensor::setDisplay(const char *color)
{
    this->getDisplayString().setTagArg("i", 1, color);
}

/**
 * Find Neighbor of sensor in transmit range
 */
void Sensor::findLinks()
{
    Sensor *s1, *s2;
    trRange = par("trRange");
    if(this->getId() == NUMTARGET + 2)
    {
        for(int i = NUMTARGET + 2; i < LastModuleId; i++)
        {
            s1 = getSensor(i);
            s1->energy = par("energy");
            s1->numNeighbor = 0;
            for(int j = NUMTARGET + 2; j < LastModuleId; j++)
            {
                s2 = getSensor(j);
                if(s2->getId() != s1->getId())
                {
                    double distance = s1->distanceCal(s2->xpos, s2->ypos);
                    if(distance <= trRange)
                    {
                        s1->trNeighbor[s1->numNeighbor] = s2;
                        s1->numNeighbor++;
                    }
                }
            }
        }
    }
}

/**
 * Create links between sensors
 */
void Sensor::createLinks(double range)
{
    Sensor *s1;
    char *outName = new char[15];
    char *inName = new char[15];
    cGate *ownGate, *otherGate;
    if(range == ssRange)
    {
        for(int i = 0; i < totalEventModule; i++)
        {
            s1 = ((Target *)tar1)->cluster[i];
            if(s1->myID != this->myID)
            {
                sprintf(outName, "O_%d", s1->getId());
                sprintf(inName, "I_%d", this->getId());
                if(!this->hasGate(outName) && !s1->hasGate(inName))
                {
                    ownGate = this->addGate(outName, cGate::OUTPUT);
                    otherGate = s1->addGate(inName, cGate::INPUT);
                    ownGate->connectTo(otherGate);
                    //hide link arow
                    ownGate->getDisplayString().setTagArg("ls", 1, "0");
                }
            }
        }
    }
    else if(range == trRange)
    {
        if(!hasLinks)
        {
            for(int i = 0; i < this->numNeighbor; i++)
            {
                s1 = this->trNeighbor[i];
                sprintf(outName, "O_%d", s1->getId());
                sprintf(inName, "I_%d", this->getId());
                if(!this->hasGate(outName) && !s1->hasGate(inName))
                {
                    ownGate = this->addGate(outName, cGate::OUTPUT);
                    otherGate = s1->addGate(inName, cGate::INPUT);
                    ownGate->connectTo(otherGate);
                    //hide link arow
                    ownGate->getDisplayString().setTagArg("ls", 1, "0");
                }

                sprintf(outName, "O_%d", this->getId());
                sprintf(inName, "I_%d", s1->getId());
                if(!s1->hasGate(outName) && !this->hasGate(inName))
                {
                    ownGate = s1->addGate(outName, cGate::OUTPUT);
                    otherGate = this->addGate(inName, cGate::INPUT);
                    ownGate->connectTo(otherGate);
                    //hide link arow
                    ownGate->getDisplayString().setTagArg("ls", 1, "0");
                }
            }
            this->hasLinks = true;
        }
    }
    else
    {
        ev <<"\n Range incorrect !";
        endSimulation();
    }
}

/**
 * Create link between 2 sensors
 */
void Sensor::createLink(Sensor *s)
{
    char *outName = new char[15];
    char *inName = new char[15];
    cGate *ownGate, *otherGate;
    sprintf(outName, "O_%d", s->getId());
    sprintf(inName, "I_%d", this->getId());
    if(!this->hasGate(outName) && !s->hasGate(inName))
    {
        ownGate = this->addGate(outName, cGate::OUTPUT);
        otherGate = s->addGate(inName, cGate::INPUT);
        ownGate->connectTo(otherGate);
        //hide link arow
        ownGate->getDisplayString().setTagArg("ls", 1, "0");
    }
}

/**
 * Delete link between 2 sensor
 */
void Sensor::deleteLink(Sensor *s)
{
    if(s == NULL) return;
    cGate *outGate;
    char outName[15], inName[15];
    sprintf(outName, "O_%d", s->myID);
    sprintf(inName, "I_%d", myID);
    if(this->hasGate(outName) && s->hasGate(inName))
    {
        outGate = gate(outName);
        outGate->disconnect();
        this->deleteGate(outName);
        s->deleteGate(inName);
    }
    this->hasLinks = false;
}

/**
 * Send Event Notify Mesage
 */
void Sensor::sendEventNotifyMessage(int totalSensorInCluster)
{
    Enter_Method_Silent("Sensor_Sefl_Message");//Co the dung duoc trong doi tuong khac
    EventNotify *emsg = new EventNotify();
    emsg->setKind(EventNotifyMsg);
    emsg->setTotalSensor(totalSensorInCluster);
    double delay;
    if(this->active)
    {
        ev <<"\n Active khi co muc tieu";
        delay = 0;
    }
    else
    {
        if(Tstart == 0 || (Tstart <= timeStep && Tstart + SDC * timeStep >= timeStep))
        {
            ev <<"\n Nut active truoc thoi diem lay mau";
            delay = 0;
            if(Tstart == 0) delay = Tdetection; //active tai thoi diem lay mau
            else if(timeStep  - Tstart < Tdetection)  //active truoc thoi diem lay mau nhung phat hien muc tieu sau thoi diem lay mau
            {
                delay = Tdetection - (timeStep - Tstart);
            }
        }
        else
        {
            delay = Tstart + Tdetection;
        }

    }
    ev <<"\n Sensor " << this->getId() - 3 <<" delay init + detection = " << delay;
    this->tDetection = delay;
    scheduleAt(simTime() + delay, emsg);
}

/**
 * Transmit data to BS
 */
void Sensor::transmitDataToBS()
{
    Enter_Method_Silent("transmitDataToBS()");
    double d = this->distanceCal(bs->xpos, bs->ypos);
    if(QueueMsg.isEmpty()) return;
    DataToBS *dataBS = (DataToBS *)QueueMsg.pop();

    if(d <= this->trRange)
    {
        ev <<"\n transmit to BS !";
        this->hasPath = true;
        dataBS->setDestAddress(bs->getId());
        initCSMA(new Package(dataBS, TRANSMIT, d));
        backOff();
    }
    else
    {
        if(typeProtocol == 0)  //ARPEES
        {
            if(this->hasPath)
            {
                d = this->distanceCal(relayNode->xpos, relayNode->ypos);
                dataBS->setDestAddress(this->relayNode->getId());
                initCSMA(new Package(dataBS, TRANSMIT, d));
                backOff();
            }
            else
            {
                ev <<"\n Find Relay Node !";
                QueueMsg.insert(dataBS);
                this->sendRelayRequest();
            }
        }
        else if(typeProtocol == 1)  //EMRP
        {
            if(this->relayEnergy >= 10.0 && this->backupEnergy >= 10.0)
            {
                if(this->relayEnergyEstimate >= 10.0)
                {
                    if(this->backupEnergy - this->relayEnergy >= (this->switchLevel * 1000 / 100))
                    {
                        Sensor *s           = this->relayNode;
                        this->relayNode     = this->backupNode;
                        this->backupNode    = s;

                        double temp         = this->relayEnergy;
                        this->relayEnergy   = this->backupEnergy;
                        this->backupEnergy  = temp;
                    }

                    d = this->distanceCal(relayNode->xpos, relayNode->ypos);
                    dataBS->setDestAddress(this->relayNode->getId());
                    initCSMA(new Package(dataBS, TRANSMIT, d));
                    backOff();
                }
                else
                {
                    QueueMsg.insert(dataBS);
                    this->sendRelayRequest();
                }

            }
            else if(this->relayEnergy >= 10.0 && this->backupEnergy < 10.0)
            {
                if(this->relayEnergyEstimate >= 10.0)
                {
                    d = this->distanceCal(relayNode->xpos, relayNode->ypos);
                    dataBS->setDestAddress(this->relayNode->getId());
                    initCSMA(new Package(dataBS, TRANSMIT, d));
                    backOff();
                }
                else
                {
                    QueueMsg.insert(dataBS);
                    this->sendRelayRequest();
                }

            }
            else if(this->relayEnergy < 10.0 && this->backupEnergy >= 10.0)
            {
                this->relayNode             = this->backupNode;
                this->relayEnergy           = this->backupEnergy;
                this->relayEnergyEstimate   = this->relayEnergy;

                this->backupNode    = NULL;
                this->backupEnergy  = 0;

                d = this->distanceCal(relayNode->xpos, relayNode->ypos);
                dataBS->setDestAddress(this->relayNode->getId());
                initCSMA(new Package(dataBS, TRANSMIT, d));
                backOff();

            }
            else if(this->relayEnergy < 10.0 && this->backupEnergy < 10.0)
            {
                QueueMsg.insert(dataBS);
                this->sendRelayRequest();
            }

        }
    }
}

/**
 * send Relay Request
 */
void Sensor::sendRelayRequest()
{
    Enter_Method_Silent("sendRelayRequest");
    this->hasPath = false;
    RelayRequest *REQ_RELAY = new RelayRequest();
    REQ_RELAY->setKind(RelayRequestMsg);
    REQ_RELAY->setSrcAddress(this->getId());
    REQ_RELAY->setLength(ControlMsgLength);
    this->msgStore = NULL;
    this->countIdError = 0;
    //remove("data/time.txt");
    //    remove("data/id1.txt");
    //    remove("data/id2.txt");

    initCSMA(new Package(REQ_RELAY, BROADCAST, trRange));
    backOff();
}

/**
 * Find relay node
 */
void Sensor::findRelayNode()
{
    Enter_Method_Silent();
    this->hasPath = false;
    double F = -1000000000;
    double djCH, djBS, dCHBS, cosa, F1;
    int count = 0;
    dCHBS = this->distanceCal(bs->xpos, bs->ypos);
    Sensor *s;
    for(int i = 0; i < numNeighbor; i++)
    {
        s = trNeighbor[i];

        if(!s->isDead)
        {
            djCH = this->distanceCal(s->xpos, s->ypos);
            djBS = s->distanceCal(bs->xpos, bs->ypos);
            cosa = (djCH * djCH + dCHBS * dCHBS - djBS * djBS)/(2 * djCH * dCHBS);

            if(cosa >= 0)
            {
                if(checkMsgError(s->getId())) continue;
                F1 = s->energy * (djCH / djBS) * cosa;
                ev <<"\n FRN cua nut " <<s->getId()-3 <<" = " << F1;
                if(F1 > F)
                {
                    count++;
                    F = F1;
                    this->relayNode = s;
                }
            }
        }
    }
    if(count != 0)
    {
        this->hasPath = true;
    }
}

void Sensor::findRelayBackupNode()
{
    Enter_Method_Silent("findRelayandBackupNode");
    this->hasPath = false;
    if(this->distanceCal(bs->xpos, bs->ypos) > trRange)
    {
        double FRN = -1000;
        double FBU = -1000;
        double djCH, djBS, dCHBS, cosa;
        Sensor *s;
        for(int i = 0; i < numNeighbor; i++)
        {
            s = trNeighbor[i];
            if(s->energy > 10)
            {
                djCH = this->distanceCal(s->xpos, s->ypos);
                djBS = s->distanceCal(bs->xpos, bs->ypos);
                dCHBS = this->distanceCal(bs->xpos, bs->ypos);
                cosa = (djCH * djCH + dCHBS * dCHBS - djBS * djBS) / (2 * djCH * dCHBS);
                if(cosa >= 0)
                {
                    if(checkMsgError(s->getId())) continue;
                    double F = s->energy * (1 / djBS) * cosa;
                    //ev <<"\n F" << s->getId() <<" = "<< F;

                    if(F > FRN)
                    {
                        FRN = F;
                        FBU = FRN;
                        this->backupNode = this->relayNode;
                        this->backupEnergy = this->relayEnergy;

                        this->relayNode = s;
                        this->relayEnergy = s->energy;
                    }

                    if(F > FBU && F <= FRN )
                    {
                        FBU = F;
                        this->backupNode = s;
                        this->backupEnergy = s->energy;
                    }
                }
            }

        }

        if(this->relayNode != NULL)
        {
            this->hasPath = true;
            this->relayEnergyEstimate = this->relayEnergy;
        }

        //        ev <<"\n Relay " << this->relayNode->getId() <<" = " << this->relayEnergy <<" ; " << this->relayNode->energy;
        //        ev <<"\n Backup " << this->backupNode->getId() <<" = " << this->backupEnergy<<" ; " << this->backupNode->energy;
    }
}

void Sensor::sendTDMA()
{
    Enter_Method_Silent("sendTDMA()");
    TDMA *tdma = new TDMA();
    tdma->setKind(TDMAMsg);
    tdma->setSrcAddress(this->getId());
    tdma->setLength(DataMsgLength);
    if(typeEstimate == 1)
    {
        if(this->hasOldInfoTarget) tdma->setIdSendInfoTarget(-1);
        else
        {
            //Chọn nút có năng lượng lớn nhất có thông tin về mục tiêu sẽ gửi về CH
            double temp = 0;
            int idSend, count = 0;
            Sensor *s;
            for(int i = 0; i < this->totalEventModule - 1; i++)
            {
                s = memberCluster[i];
                if(s->active && s->hasOldInfoTarget)
                {
                    if(s->energy > temp)
                    {
                        temp = s->energy;
                        idSend = s->getId();
                        count++;
                    }
                }
            }

            if(count > 0) tdma->setIdSendInfoTarget(idSend);
            else tdma->setIdSendInfoTarget(-1);
        }
    }

    int count = 0;
    for(int i = 0; i < this->totalEventModule - 1; i++)
    {
        if(memberCluster[i]->active)
        {
            tdma->setId(count, memberCluster[i]->myID);
            tdma->setTimeSlot(count, count + 1);
            //ev <<"\n " << memberCluster[i]->myID <<" : " << count + 1;
            count++;
        }
    }

    initCSMA(new Package(tdma, BROADCAST, ssRange * 2));
    backOff();
}

void Sensor::sendSeflMsgToCH()
{
    Enter_Method_Silent("sendSeflMsgToCH()");
    CHSELF *chSefl = new CHSELF();
    chSefl->setKind(CHSelfMsg);
    scheduleAt(simTime(), chSefl);
}

void Sensor::scheduleAtInit()
{
    Enter_Method_Silent("scheduleAtInit");
    cMessage *initMsg = new cMessage();
    initMsg->setKind(SMSG_INIT);
    scheduleAt(simTime(), initMsg);
}

/**
 * Caculation distance between current sensor and module has position (x, y)
 */
double Sensor::distanceCal(double x, double y)
{
    double xlength = this->xpos - x;
    double ylength = this->ypos - y;
    double distance = sqrt(xlength * xlength + ylength * ylength);
    return distance;
}

/**
 * Elect Cluster head
 */
void Sensor::electCH()
{

    double F = this->energy * this->infoTarget;
    this->clusterHead = this;
    for(int i = 0; i < this->totalEventModule - 1; i++)
    {
        Sensor *s = this->memberCluster[i];
        if(s->active == true)
        {
            double F1 = s->energy * s->infoTarget;
            if(F1 > F)
            {
                this->clusterHead = s;
                F = F1;
            }
        }
    }

    if(this->clusterHead->getId() == myID)
    {
        this->setDisplay("blue");
        this->isClusterHead = true;
        this->sendTDMA();
    }
}

/**
 * Get Sensor in array trNeighbor[]
 */
Sensor* Sensor::getSensor(int id)
{
    return (Sensor *)simulation.getModule(id);
}

/**
 * Calculate the remaining energy after transmission
 */
void Sensor::energySend(int bits, double d)
{
    double en;
    if(d <= this->trRange)
    {
        en = (Eelec + Efs * d * d) * bits;
    }
    else
    {
        en = (Eelec + 0.0013 * 0.000001 * d * d * d * d) * bits;
    }
    this->energy = this->energy - en;
    if(this->energy < 10.0)
    {
        ev <<"\n Nang luong cua sensor chet "<<this->getId()-3<<" la: " << this->energy;
        this->active = false;
        this->isDead = true;
        this->setDisplay("black");
    }
}

/**
 * Calculate the remaining energy after Receive
 */
void Sensor::energyReceive(int bits)
{
    Enter_Method_Silent("energyReceive");
    this->energy -= Eelec * bits;
    if(this->energy < 10.0)
    {
        ev <<"\n Nang luong cua sensor chet "<<this->getId()-3<<" la: " << this->energy;
        this->active = false;
        this->isDead = true;
        this->setDisplay("black");
    }
}

/**
 * Estimate Energy of relay Node
 * Nang luong mat = nang luong nhan va truyen msg DataToBS
 */
void Sensor::estimateEnergyRelay()
{
    if(this->relayNode != NULL)
    {
        double d = trRange;

        //Vị trí relayNode đã biết
        if(this->relayNode->distanceCal(bs->xpos, bs->ypos) <= trRange)
        {
            d = this->relayNode->distanceCal(bs->xpos, bs->ypos);
        }

        this->relayEnergyEstimate -= (Eelec * DataMsgLength + (Eelec + Efs * d * d) * DataMsgLength);

        //        ev <<"\n Energy Relay (store) " << this->relayNode->getId() - (NUMTARGET + 2)<<": = " << this->relayEnergy;
        //        ev <<"\n Energy Backup (store) " << this->backupNode->getId() - (NUMTARGET + 2)<<": = " << this->backupEnergy;
        //        ev <<"\n Energy Relay Estimate " << this->relayNode->getId() - (NUMTARGET + 2)<<": = "  << this->relayEnergyEstimate;

    }
}

/**
 * Lateration
 * Locate the target
 * A * tarPos = B
 * => tarPos = (A^T * A)^-1 * A^T * B
 */
void Sensor::locateTarget()
{
    if(totalEventModule < 3) return;

    Matrix tarPos = Matrix(2, 1);
    Matrix A = Matrix(this->totalEventModule - 1, 2);
    Matrix B = Matrix(this->totalEventModule - 1, 1);
    Sensor *s;
    int x1 = this->xpos, y1 = this->ypos, r1 = this->R_measurement;
    int xi, yi, ri;
    for(int i = 0; i < this->totalEventModule - 1; i++)
    {
        s = this->memberCluster[i];
        xi = s->xpos;
        yi = s->ypos;
        ri = s->R_measurement;
        //set value of Matrix A
        A.setData(i, 0, 2 * (x1 - xi));
        A.setData(i, 1, 2 * (y1 - yi));

        //set value of Matrix B

        B.setData(i, 0, (ri*ri-r1*r1) - (xi*xi-x1*x1) - (yi*yi-y1*y1));
    }

    tarPos = (A.transpose() * A).inverse() * A.transpose() * B;
    double x = tarPos.getData(0, 0);
    double y = tarPos.getData(1, 0);
    if(x < 0) x = 0;
    if(y < 0) y = 0;
    this->X.setData(0, 0, x);  //x
    this->X.setData(2, 0, y);  //y

    ev <<"\n Estimate Lateration: ";
    X.Output();
}

/**
 * Estimate postion of target with Extend Kalman Filter
 */

void Sensor::estimatesTarget()
{
    if(!hasOldInfoTarget)
    {
        ev <<"\n Has not Info Target";
        this->X.setData(0, 0, this->xpos);
        this->X.setData(1, 0, 9);                     //vx
        this->X.setData(2, 0, this->ypos);
        this->X.setData(3, 0, 0);                     //vy
        KF.setX(this->X);
        KF.Predict();

        ev <<"\n Measure: " << KF.getX().getData(0, 0) <<" ; " << KF.getX().getData(2, 0);
        ev <<"\n Du doan: " << KF.getX0().getData(0, 0) <<" ; " << KF.getX0().getData(2, 0);
    }
    else
    {
        ev <<"\n Has Info Target";
        Matrix H = Matrix(totalEventModule, 4, 0);
        Matrix Z = Matrix(totalEventModule, 1);
        Matrix Z_Pre = Matrix(totalEventModule, 1);

        ev <<"\n X_Old: "; KF.getX().Output();
        ev <<"\n P_Old: "; KF.getP().Output();
        KF.Predict();

        double x0 = KF.getX0().getData(0, 0);
        double y0 = KF.getX0().getData(2, 0);
        double d = distanceCal(x0, y0);
        Z.setData(0, 0, R_measurement);
        Z_Pre.setData(0, 0, d);
        double h0 = (x0 - this->xpos) / d;
        //h1, h3 = 0;
        double h2 = (y0 - this->ypos) / d;
        H.setData(0, 0, h0); H.setData(0, 2, h2);
        //ev <<"\n d = " << d;
        Sensor *s;
        for(int i = 0; i < totalEventModule - 1; i++)
        {
            s = memberCluster[i];
            d = s->distanceCal(x0, y0);
            Z.setData(i + 1, 0, s->R_measurement);
            Z_Pre.setData(i + 1, 0, d);
            h0 = (x0 - s->xpos) / d;
            h2 = (y0 - s->ypos) / d;
            //ev <<" ; " << d;
            H.setData(i + 1, 0, h0); H.setData(i + 1, 2, h2);
        }
        ev <<"\n X0: "; KF.getX0().Output();
        ev <<"\n H: "; H.Output();
        ev <<"\n Z: "; Z.Output();
        ev <<"\n Z^: "; Z_Pre.Output();

        KF.setH(H);
        KF.setZ_Pre(Z_Pre);
        Matrix R = Matrix(totalEventModule, totalEventModule, 1);
        R = R * sigmaMeasurementNoise * sigmaMeasurementNoise;
        KF.setR(R);

        KF.Update(Z);

        this->X = KF.getX();

        if(this->X.getData(0, 0) < 0) this->X.setData(0, 0, 0);
        if(this->X.getData(2, 0) < 0) this->X.setData(2, 0, 0);
        KF.setX(this->X);

        ev <<"\n True: " << ((Target *)tar1)->xpos <<" ; " << ((Target *)tar1)->ypos;
        ev <<"\n Du doan: " << KF.getX0().getData(0, 0) <<" ; " << KF.getX0().getData(2, 0);
        ev <<"\n Kalman: " << KF.getX().getData(0, 0) <<" ; " << KF.getX().getData(2, 0);
        KF.Predict();
        //ev <<"\n Predict: " << KF.getX0().getData(0, 0) <<" ; " << KF.getX0().getData(2, 0);
    }
}

Matrix Sensor::getCovarianceMatrix(double covarianceArray[])
{
    Matrix P = Matrix(4, 4, 1);
    int count = 0;
    for(int i = 0; i < 4; i++)
        for(int j = 0; j < 4; j++)
        {
            if(j >= i)
            {
                P.setData(i, j, covarianceArray[count]);
                P.setData(j, i, covarianceArray[count]);
                count++;
            }
        };
    return P;
}

void Sensor::broadcastTargetInfo(double distance)
{
    TargetInfo *targetInfoMsg = new TargetInfo();
    targetInfoMsg->setKind(TargetInfoMsg);
    targetInfoMsg->setSrcAddress(this->getId());
    targetInfoMsg->setLength(DataMsgLength);
    targetInfoMsg->setValue(t_start_sampling);
    for(int i = 0; i < 4; i++)
    {
        targetInfoMsg->setX(i, X.getData(i, 0));
    }

    int count = 0;
    for(int i = 0; i < 4; i++)
        for(int j = 0; j < 4; j++)
        {
            if(j >= i)
            {
                targetInfoMsg->setP(count, P.getData(i, j));
                count++;
            }
        };

    initCSMA(new Package(targetInfoMsg, BROADCAST, distance));
    backOff();

}

void Sensor::resetDisplay()
{
    Enter_Method_Silent("resetDisplay()");
    if(this->gateCount() > 0)
    {
        Sensor *s;
        cGate *outGate;
        char outName[15], inName[15];

        //remove gate
        for(int i = 0; i < numNeighbor; i++)
        {
            s = this->trNeighbor[i];
            if(s->energy < 10.0)
            {
                s->active = false;
                s->isDead = true;
                s->setDisplay("black");
            }
            s->isClusterHead = false;
            sprintf(outName, "O_%d", s->myID);
            sprintf(inName, "I_%d", myID);
            if(this->hasGate(outName) && s->hasGate(inName))
            {
                outGate = gate(outName);
                outGate->disconnect();
                this->deleteGate(outName);
                s->deleteGate(inName);
            }

            sprintf(outName, "O_%d", this->myID);
            sprintf(inName, "I_%d", s->myID);
            if(s->hasGate(outName) && this->hasGate(inName))
            {
                outGate = s->gate(outName);
                outGate->disconnect();
                s->deleteGate(outName);
                this->deleteGate(inName);
            }
        }
        this->hasLinks = false;
    }
    this->CSMAQueueMsg.clear();
    this->totalEventModule = 0;
    this->totalStatusInfoMsg = 0;
    this->frameNumber = par("frameNumber");
    this->isClusterHead = false;
    if(this->energy < 10.0)
    {
        this->isDead = true;
        this->setDisplay("black");
    }
}

void Sensor::backOff()
{
    double tBO = (rand() % (int)(pow(2, BE) - 1)) * aUnitBackoffPeriod;
    //ev <<"\n Sensor " << this->getId() - (NUMTARGET + 2) <<" BE  = " << BE <<"; BackOff = " << tBO;
    BackOFF *backOff = new BackOFF();
    backOff->setKind(BackOffSeflMsg);
    scheduleAt(simTime() + tBO, backOff);
}

void Sensor::initCSMA(Package *package)
{
    ev <<"\n Sensor " << this->getId() - (NUMTARGET + 2) <<" Init CSMA: QueueMsg";
    this->NB = 0;
    this->BE = macMinBE;
    CSMAQueueMsg.insert(package);
}

void Sensor::clearChannelAssessment()
{
    if(this->flag == 0)
    {
        if(CSMAQueueMsg.isEmpty()) return;
        Package *package = (Package *)CSMAQueueMsg.pop();
        SensorMessage *msg = package->getMessage();
        double distance = package->getDistance();

        int packageLength = msg->getLength();
        double timer;
        if(packageLength <= 18 * 8)
        {
            timer = packageLength / bitRate + aMinSIFSPeriod;
        }
        else timer = packageLength / bitRate + aMinLIFSPeriod;

        CleanChanel *cleanChanelMsg = new CleanChanel();
        cleanChanelMsg->setKind(CleanChanelSeflMsg);
        cleanChanelMsg->setDistance(distance);
        this->chiemKenh(distance);

        scheduleAt(simTime() + timer, cleanChanelMsg);

        char gateName[15];
        if(strcmp(package->getType(), BROADCAST) == 0)
        {
            if(!hasLinks) createLinks(trRange);
            ev <<"\n Sensor " << this->getId() - (NUMTARGET + 2) <<" Broadcast";
            Sensor *s;

            if(msg->getKind() == RelayRequestMsg)
            {
                timeoutREQ_Relay = new TimeOut();
                timeoutREQ_Relay->setKind(TimeOutReqRelayMsg);
                scheduleAt(simTime() + timer + timeOutReceiveAck, timeoutREQ_Relay);
            }

            for(int i = 0; i < numNeighbor; i++)
            {
                s = trNeighbor[i];
                if(s->energy >= 10)
                {
                    if(msg->getKind() == RelayRequestMsg)
                    {
                        sprintf(gateName, "O_%d", s->getId());
                        if(this->hasGate(gateName)) sendDelayed(msg->dup(), timer, gateName);
                    }
                    else if(s->distanceCal(xpos, ypos) <= distance)
                    {
                        sprintf(gateName, "O_%d", s->getId());
                        if(msg->getKind() == StatusInfoMsg || msg->getKind() == TDMAMsg)
                        {
                            if(s->active) //Gui cho nhung nut trong cum
                            {
                                if(this->hasGate(gateName)) sendDelayed(msg->dup(), timer, gateName);
                            }
                        }
                        else
                        {
                            if(this->hasGate(gateName)) sendDelayed(msg->dup(), timer, gateName);
                        }
                    }
                }
                else
                {
                    s->active = false;
                    s->isDead = true;
                    s->setDisplay("black");
                }
            }
            this->energySend(packageLength, distance);
        }
        else
        {
            int idDest = msg->getDestAddress();
            if(idDest != bs->getId()) this->createLink(getSensor(idDest));
            ev <<"\n Sensor " << this->getId() - (NUMTARGET + 2) <<" Transmit";
            sprintf(gateName, "O_%d", msg->getDestAddress());
            if(this->hasGate(gateName))
            {
                if(msg->getKind() != AckRelayMsg && idDest != bs->getId())
                {
                    cGate *g = gate(gateName);
                    g->getDisplayString().parse("ls=red,1");
                }
                sendDelayed(msg->dup(), timer, gateName);
            }
            this->energySend(packageLength, distance);

            if(msg->getKind() == DataToBSMsg)
            {
                //EKF
                if(typeEstimate == 1 && this->isClusterHead)
                {
                    double dCHtoTarget = this->distanceCal(KF.getX0().getData(0, 0), KF.getX0().getData(2, 0));
                    ev <<"\n dCHtoTarget = " << dCHtoTarget;

                    NotifySendInfoTarget *notifyMsg = new NotifySendInfoTarget();
                    notifyMsg->setKind(NotifySeflMsg);
                    scheduleAt(simTime() + timer, notifyMsg);
                }

                if(typeEstimate == 0 && this->isClusterHead)
                {
                    int numSensor;
                    if(speed * timeStep >= 2 * ssRange)
                    {
                        numSensor = 10;
                    }
                    else
                    {
                        numSensor = 5;
                        if(this->totalEventModule <= numSensor)
                        {
                            Active *activeMsg = new Active();
                            activeMsg->setKind(CHNotifyActiveMsg);
                            scheduleAt(simTime() + timer, activeMsg);
                        }
                    }
                }


                //EMRP
                if(typeProtocol == 1)
                {
                    ev <<"\n ---> Energy (true) " << this->getId() - (NUMTARGET + 2) <<": = "  << this->energy<<"\n\n\n";
                    estimateEnergyRelay();
                    if(countPackageTransmit == 1)
                    {
                        int idParent = ((DataToBS *)msg)->getParentAdress();
                        Sensor *s = getSensor(idParent);
                        double d = s->distanceCal(xpos, ypos);
                        EnergyRelayInfo * energyRelay = new EnergyRelayInfo();
                        energyRelay->setKind(EnergyRelayInfoMsg);
                        energyRelay->setSrcAddress(this->getId());
                        energyRelay->setDestAddress(idParent);
                        energyRelay->setLength(ControlMsgLength);
                        energyRelay->setEnergy(this->energy);
                        initCSMA(new Package(energyRelay, TRANSMIT, d));
                        backOff();
                        countPackageTransmit = 0;
                        if(idDest != bs->getId())
                        {
                            timeoutEnergy_Relay =  new TimeOut();
                            timeoutEnergy_Relay->setKind(TimeOutEnergyRelay);
                            double timeCSMA = pow(2, 5) * aUnitBackoffPeriod + CCAduration;
                            double timeOut = 2 * timer + 2 * timeCSMA + ControlMsgLength / bitRate;
                            ev <<"\n TimeOut Energy relay = " << timeOut;
                            scheduleAt(simTime() + timeOut, timeoutEnergy_Relay);
                        }
                    }
                }
            }
        }
    }
    else
    {
        ev << "\n Sensor "<< this->getId() - 2 <<" check: Channel not idle";
        this->NB++;
        this->BE = (BE + 1) > aMaxBE ? aMaxBE : BE + 1;
        if(NB <= macMaxCSMABackoffs) backOff();
        else
        {
            ev <<"\n Failure !";
            CSMAQueueMsg.clear();
        }
    }
}
void Sensor::chiemKenh(double distance)
{
    Enter_Method_Silent("ChiemKenh");
    ev <<"\n Sensor " << this->getId() - (NUMTARGET + 2) <<" Chiem kenh";
    this->flag = 1;
    Sensor *s;
    for(int i = 0; i < numNeighbor; i++)
    {
        s = trNeighbor[i];
        if(distance == trRange) s->flag = 1;
        else if(this->distanceCal(s->xpos, s->ypos) <= distance)
        {
            s->flag = 1;
        }
    }
}

void Sensor::giaiPhongKenh(double distance)
{
    Enter_Method_Silent("GaiPhongKenh");
    ev <<"\n Sensor " << this->getId() - (NUMTARGET + 2) <<" Giai phong kenh";
    this->flag = 0;
    Sensor *s;
    for(int i = 0; i < numNeighbor; i++)
    {
        s = trNeighbor[i];
        if(distance == trRange) s->flag = 0;
        else if(this->distanceCal(s->xpos, s->ypos) <= distance) s->flag = 0;
    }
}

void Sensor::storeIdError(int idPrevMsg, int idCurrMsg)
{
    int count = 0;
    for(int i = 0; i < countIdError; i++)
    {
        if(idPrevMsg == idError[i])
        {
            count++;
            break;
        }
    }

    if(count == 0)
    {
        idError[countIdError] = idPrevMsg;
        countIdError++;
        //bs->storeValue(idPrevMsg, "data/id1.txt");
    }

    idError[countIdError] = idCurrMsg;
    countIdError++;
    //bs->storeValue(idCurrMsg, "data/id1.txt");
}

bool Sensor::checkMsgError(int idSender)
{
    bool isError = false;
    for(int i = 0; i < countIdError; i++)
    {
        if(idSender == idError[i])
        {
            isError = true;
            break;
        }
    }
    return isError;
}


